#include <buola/buola.h>

#if BUOLA_PLATFORM_LINUX

#include "cwebcamdevice.h"
#include "cwebcamdriver.h"

#include <buola/image/format.h>
#include <buola/image/io.h>
#include <fcntl.h>
#include <linux/videodev2.h>
#include <sys/ioctl.h>

namespace buola { namespace cvi {

CWebCamDevice::CWebCamDevice(const io::CURI &pDevice)
    :   mInitialized(false)
    ,   mDevice(pDevice)
    ,   mSyncIndex(0)
    ,   mStartCount(0)
    ,   mStarted(false)
{
}

CWebCamDevice::~CWebCamDevice()
{
    
}

void CWebCamDevice::Start()
{
    if(!mStartCount++)
    {
        lock_guard<mutex> lLock(mMutex);
        if(mThread.get_id()==thread::id())
            mThread=thread([this](){OnThread();});
        mCondVar.notify_one();
    }
}

void CWebCamDevice::Stop()
{
    mStartCount--;
}

img::CImage_rgb8 CWebCamDevice::GetOne()
{
    img::CImage_rgb8 lResult;
    Start();
    {
        unique_lock<mutex> lLock(mSyncMutex);
        int lSyncIndex=mSyncIndex;
        while(lSyncIndex==mSyncIndex)
            mSyncCondVar.wait(lLock);
        lResult=share(mSyncImage);
    }
    Stop();
    
    return lResult;
}

void CWebCamDevice::IOCtl(long int pRequest,void* pPtr)
{
    if(ioctl(mFD,pRequest,pPtr)<0)
        throw XIO("ioctl failed for webcam: "+c_error_string(errno));
}

bool CWebCamDevice::IOCtl(std::nothrow_t,long int pRequest,void* pPtr)
{
    return ioctl(mFD,pRequest,pPtr)>=0;
}

std::vector<uint32_t> CWebCamDevice::EnumerateFormats(uint32_t pType)
{
    std::vector<uint32_t> lFormats;
    v4l2_fmtdesc lDesc;
    zero_pod(lDesc);
    lDesc.type=pType;
    for(lDesc.index = 0;lDesc.index<100;lDesc.index++) //safe limit in case driver is broken
    {
        if(!IOCtl(std::nothrow,VIDIOC_ENUM_FMT,&lDesc))
            break;
        lFormats.push_back(lDesc.pixelformat);
    }
    
    return lFormats;
}

CWebCamDevice::SFormat CWebCamDevice::QueryFormat(uint32_t pType)
{
    v4l2_format lFmt;
    zero_pod(lFmt);
    lFmt.type=pType;
    IOCtl(VIDIOC_G_FMT,&lFmt);

    SFormat lFormat;
    lFormat.mPixel=lFmt.fmt.pix.pixelformat;
    lFormat.mSize=CSize_i(lFmt.fmt.pix.width,lFmt.fmt.pix.height);
    lFormat.mInterlaced=(lFmt.fmt.pix.field!=V4L2_FIELD_NONE);
    lFormat.mStride=lFmt.fmt.pix.bytesperline;
    lFormat.mTotalBytes=lFmt.fmt.pix.sizeimage;
    
    msg_info() << "current format:" << lFormat.mPixel << " " << lFormat.mSize << " " << lFormat.mInterlaced << " " <<
                  lFormat.mStride << " " << lFormat.mTotalBytes << "\n";
    
    return lFormat;
}

void CWebCamDevice::ResetCrop(uint32_t pType)
{
    /* check cropping */
    v4l2_cropcap lCCap;
    zero_pod(lCCap);
    lCCap.type=pType;
    IOCtl(VIDIOC_CROPCAP,&lCCap);

    /* reset crop parameters */
    v4l2_crop lCrop;
    zero_pod(lCrop);
    lCrop.type=pType;
    lCrop.c=lCCap.defrect;
    if(!IOCtl(std::nothrow,VIDIOC_S_CROP,&lCrop))
        msg_warn() << "can't reset crop\n";
}

void CWebCamDevice::SetIOMode(uint32_t pType)
{
    v4l2_requestbuffers lRB;
    zero_pod(lRB);
    lRB.type=pType;
    lRB.count=4; ///\todo no magic constant
    lRB.memory=V4L2_MEMORY_USERPTR; ///\todo use mmap if possible
    
    IOCtl(VIDIOC_REQBUFS,&lRB);

    if(lRB.memory!=V4L2_MEMORY_USERPTR)
        throw XIO("can't set io mode");
    
    msg_info() << "actual buffers: " << lRB.count << "\n";
}

void CWebCamDevice::EnqueueBuffer(uint32_t pType, char* pBuffer,int pIndex)
{
    v4l2_buffer lBuf;
    zero_pod(lBuf);
    lBuf.type=pType;
    lBuf.memory=V4L2_MEMORY_USERPTR;
    lBuf.m.userptr=(unsigned long)pBuffer;
    lBuf.length=mCurrentFormat.mTotalBytes;
    lBuf.index=pIndex;

    IOCtl(VIDIOC_QBUF,&lBuf);
}

void CWebCamDevice::Initialize()
{
    lock_guard<mutex> lLock(mMutex);
    if(mInitialized) return;

    mFD=open("/dev/video0",O_RDWR);
    
    v4l2_capability lVCap=IOCtl<v4l2_capability>(VIDIOC_QUERYCAP);
    
    if(!(lVCap.capabilities&V4L2_CAP_VIDEO_CAPTURE)||!(lVCap.capabilities&(V4L2_CAP_READWRITE|V4L2_CAP_STREAMING)))
        throw XInvalid("missing capabilities in web camera");
    
    msg_info() << "resetting crop\n";
    ResetCrop(V4L2_BUF_TYPE_VIDEO_CAPTURE);

    std::vector<uint32_t> lFormats=EnumerateFormats(V4L2_BUF_TYPE_VIDEO_CAPTURE);
    if(lFormats.empty())
        throw XIO("can't enumerate video formats");

    mCurrentFormat=QueryFormat(V4L2_BUF_TYPE_VIDEO_CAPTURE);
    //now we could set it, it's fine for now.
    
    //set io mode
    SetIOMode(V4L2_BUF_TYPE_VIDEO_CAPTURE);
    
    for(int i=0;i<4;i++)
    {
        char *lBuffer=alloc_array<char>(mCurrentFormat.mTotalBytes);
        EnqueueBuffer(V4L2_BUF_TYPE_VIDEO_CAPTURE,lBuffer,i);
    }
    
    msg_info() << "initialization complete\n";
    
    mInitialized=true;
}

void CWebCamDevice::OnThread()
{
    msg_info() << "onthread\n";
    try
    {
        int lFrameCount=0;
        
        while(true)
        {
            if(!mStartCount)
            {
                lFrameCount=0;
                
                unique_lock<mutex> lLock(mMutex);
                
                while(!mStartCount)
                {
                    if(mStarted)
                        DoStop();
                    
                    mCondVar.wait(lLock);
                }
            }
            
            if(!mStarted)
            {
                DoStart();
            }
            
            DoIteration(++lFrameCount);
        }
    }
    catch(std::exception &pE)
    {
        msg_info() << "got exception in thread " << pE.what() << "\n";
    }
}

void CWebCamDevice::DoStart()
{
    if(!mInitialized) Initialize();
    
    msg_info() << "starting!!\n";
    v4l2_buf_type lType=V4L2_BUF_TYPE_VIDEO_CAPTURE;
    IOCtl(VIDIOC_STREAMON,&lType);
    msg_info() << "started\n";
    
    mStarted=true;
}

void CWebCamDevice::DoStop()
{
    msg_info() << "stopping!!\n";
    v4l2_buf_type lType=V4L2_BUF_TYPE_VIDEO_CAPTURE;
    IOCtl(VIDIOC_STREAMON,&lType);
    msg_info() << "stopped\n";

    mStarted=false;
}

void CWebCamDevice::DoIteration(int pCount)
{
    v4l2_buffer lBuf;
    zero_pod(lBuf);
    lBuf.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
    lBuf.memory=V4L2_MEMORY_USERPTR;
    IOCtl(VIDIOC_DQBUF,&lBuf);
    
    img::CImage_rgb8 lImage=img::load_mem<img::CImage_rgb8>((uint8_t*)lBuf.m.userptr,lBuf.bytesused,"jpg");
    EnqueueBuffer(V4L2_BUF_TYPE_VIDEO_CAPTURE,(char*)lBuf.m.userptr,lBuf.index);

    lImage.TimeStampNow();
     
    sImage(lImage);
 
    if(pCount>10)
    {
        lock_guard<mutex> lLock(mSyncMutex);
     
        mSyncIndex++;
        mSyncImage=std::move(lImage);
        mSyncCondVar.notify_all();
    } 
    else
    {
        msg_info() << "ignoring one\n";
    }
}

/*namespace cvi*/ } /*namespace buola*/ }

#endif
